﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Windows.Devices.Geolocation;
using Windows.Devices.Sensors;
using System.Diagnostics;
using Windows.Devices.Sensors;


namespace Bluefly
{
    

    public class PositionService
    {


        public event EventHandler<PositionEventArgs> PositionUpdated;

        public Position Position
        {
            get;
            private set;

        }

        Geolocator locator;
        Accelerometer accelerometer;
        SimpleOrientationSensor orientationSensor;

        public KalmanFilter _filter;

        public PositionService()
        {

            locator = new Geolocator();
            locator.MovementThreshold = 2;
            locator.ReportInterval = 200;
            locator.DesiredAccuracyInMeters = 5;
            locator.DesiredAccuracy = PositionAccuracy.High;
            locator.PositionChanged += locator_PositionChanged;
            locator.StatusChanged += locator_StatusChanged;


            /*
             accelerometer = Windows.Devices.Sensors.Accelerometer.GetDefault();
             accelerometer.ReadingChanged += PositionService_ReadingChanged;
             var k =  accelerometer.MinimumReportInterval;
             accelerometer.ReportInterval = 20;
            */
        }

        void orientationSensor_OrientationChanged(Windows.Devices.Sensors.SimpleOrientationSensor sender, Windows.Devices.Sensors.SimpleOrientationSensorOrientationChangedEventArgs args)
        {
            
        }

        void orientationSensor_ReadingChanged(Windows.Devices.Sensors.OrientationSensor sender, Windows.Devices.Sensors.OrientationSensorReadingChangedEventArgs args)
        {
            Debug.WriteLine(args.Reading.RotationMatrix.M11);      
        }


        double velocityX;
        double positionX;
        double positionLast;
        double accelLast;
        double sumX;
        
        long samples = 0;
        double zf;

        double time = 0.02D;

        void PositionService_ReadingChanged(Accelerometer sender, AccelerometerReadingChangedEventArgs args)
        {
            
            samples++;

            var currentX = args.Reading.AccelerationX;
            var currentY = args.Reading.AccelerationY;
            var currentZ = args.Reading.AccelerationZ;

            velocityX = (accelLast * time) + (time * 0.5D * (accelLast - currentX));


            positionX = positionLast + velocityX * time;

            accelLast = currentX;
            positionLast = positionX;

            //Debug.WriteLine("{2:0.0000000}, v ={0}, p={1}", velocityX, positionX, current);

            Debug.WriteLine("{0}\t{1}\t{2}", currentX, currentY, currentZ);


            var t = Tuple.Create<double,double,double,DateTimeOffset>(args.Reading.AccelerationX, args.Reading.AccelerationY, args.Reading.AccelerationZ, args.Reading.Timestamp);


    
          

        }

        void locator_StatusChanged(Geolocator sender, StatusChangedEventArgs args)
        {
            
        }

        void locator_PositionChanged(Geolocator sender, PositionChangedEventArgs args)
        {
            lock (this)
            {

                var coord = args.Position.Coordinate; 
                var pos = coord.Point.Position;
                Position = new Position()
                {
                    AltitudeAccuracy = coord.AltitudeAccuracy,
                    Latitude = pos.Latitude,
                    Accuracy = coord.Accuracy,
                    Altitude = pos.Altitude,
                    Longitude = pos.Longitude,
                    Velocity = coord.Speed,
                    Heading = coord.Heading,
                    Source = (Bluefly.PositionSource)coord.PositionSource,
                    Timestamp = coord.Timestamp
                };

            }

            Debug.WriteLine(Position.Velocity);

            var evt = this.PositionUpdated;
            if (evt != null)
                evt(this, new PositionEventArgs(this.Position));

        }

    

    }
}
